#include <common/event_publisher.h>


namespace common
{

EventPublisher::EventPublisher()
{
}


EventPublisher::~EventPublisher()
{
	event_timer_->cancel();
	events_.clear();
	events_.resize(0);
}

void EventPublisher::init(rclcpp::Node::SharedPtr node)
{
	node_ = node;	
	events_.clear();
	events_.resize(0);
	last_publish_time_ = node_->now();

	
	event_timer_ = node_->create_wall_timer(std::chrono::duration<double>(0.1), 
		std::bind(&EventPublisher::eventtimer_callback, this));
    events_pub_ = node_->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1);
}

void EventPublisher::add(agv_msgs::msg::AGVEvent &event, int unique_index, int unique_value)
{
	if (unique_index) {
		rclcpp::Time timestamp = node_->now();
		bool existed = false;

		for (auto &e : events_) {
			switch (unique_index) {
				case 1:
					// RCLCPP_INFO(node_->get_logger(), "[EventPublisher] index: %d  param1: %d  value: %d", unique_index, e.param1, unique_value);
					if (e.param1 == unique_value) {
						existed = true;
						e = event;
						e.eventtime = timestamp.seconds();
					}
					break;
				case 2:
					// RCLCPP_INFO(node_->get_logger(), "[EventPublisher] index: %d  param2: %d  value: %d", unique_index, e.param2, unique_value);
					if (e.param2 == unique_value) {
						existed = true;
						e = event;
						e.eventtime = timestamp.seconds();
					}
					break;
				case 3:
					// RCLCPP_INFO(node_->get_logger(), "[EventPublisher] index: %d  param3: %d  value: %d", unique_index, e.param3, unique_value);
					if (e.param3 == unique_value) {
						existed = true;
						e = event;
						e.eventtime = timestamp.seconds();
					}
					break;
				case 4:
					// RCLCPP_INFO(node_->get_logger(), "[EventPublisher] index: %d  param4: %d  value: %d", unique_index, e.param4, unique_value);
					if (e.param4 == unique_value) {
						existed = true;
						e = event;
						e.eventtime = timestamp.seconds();
					}
					break;
				default:
					break;
			}
		}

		if (!existed) {
			RCLCPP_INFO_STREAM(node_->get_logger(), "[EventPublisher] add event: " << event);
			events_.push_back(event);
			RCLCPP_INFO(node_->get_logger(), "[EventPublisher] events size: %ld", events_.size());
		}
	}
}

void EventPublisher::clear()
{
	events_.clear();
	events_.resize(0);
}


void EventPublisher::clear(int unique_index, int unique_value)
{
	if (unique_index) {
		events_.erase(
			std::remove_if(events_.begin(), events_.end(), [&](const agv_msgs::msg::AGVEvent& event) 
				{
				bool match = false;
				switch (unique_index) {
					case 1:
						match = event.param1 == unique_value;
						break;
					case 2:
						match = event.param2 == unique_value;
						break;
					case 3:
						match = event.param3 == unique_value;
						break;
					case 4:
						match = event.param4 == unique_value;
						break;
					default:
						break;
				}
				if (match) {
					RCLCPP_INFO(node_->get_logger(), "[EventPublisher] delete event: %d", unique_value);
				}
				return match;
				}
			), events_.end());

		// RCLCPP_INFO_THROTTLE(1.0, "[EventPublisher] clear. events size: %d", events_.size());
	}
}


void EventPublisher::eventtimer_callback()
{
	RCLCPP_INFO_THROTTLE(node_->get_logger(), *(node_->get_clock()), 
			1000.0, "[EventPublisher] events size: %ld", events_.size());

	rclcpp::Time timestamp = node_->now();
	if (events_.size() > 0) {
		for (auto &e : events_) {
			e.header.stamp = timestamp;
			e.header.frame_id = node_->get_name();
			events_pub_->publish(e);
		}
	}
	else {
		if ((timestamp - last_publish_time_).seconds() > 1.0) {
			agv_msgs::msg::AGVEvent event;
			event.header.stamp = timestamp;
			event.header.frame_id = node_->get_name();
			event.ecode = agv_msgs::msg::AGVEvent::NONE_EVENT;
			event.type = agv_msgs::msg::AGVEvent::INFO;
			event.action = agv_msgs::msg::AGVEvent::NONE;
			event.ackreq = true;
			event.param1 = 0;
			event.param2 = 0;
			event.description = "No event";
			events_pub_->publish(event);
			last_publish_time_ = timestamp;	
		}
		
	}
}

}